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1. Introduction 


A precise specification of the trajectory of the end effector is a prerequisite towards successful 
application of a manipulator to many tasks. Arc welding, spray painting, conveyor belt tracking, 
and glueing are some tasks which require specification of both the spatial and temporal aspects 
of a trajectory. In the most general case, not only the Cartesian position and velocity of the end 
effector, or hand, must be specified, but also the Cartesian accelerations. 

The transformation of a Cartesian trajectory of the hand into the corresponding joint angle 
trajectory of the manipulator, the so-called inverse kinematics problem, has been studied primarily 
in the context of positions and velocities. Reasonably efficient algorithms have been developed 
for these transformations (Paul 1981. Featherstone 1983), yet little attention has been paid to the 
solution of tire inverse kinematic accelerations. 

In this paper, we present an efficient algorithm for the calculation of the inverse kinematic 
accelerations for a 6 degree-of-freedom (dof) manipulator with a spherical wrist, based on a 
technique developed by Featherstone (1983) for inverse kinematic positions and velocities. In 
addition, we show that the inverse kinematic calculations work syncrgistically with the inverse 
dynamic calculations, because the extended Featherstone method yields kinematic parameters 
needed in the backward recursion steps of the Newton-Euler dynamics formulation (Luh, Walker, 
and Paul 1980a). We note that consistency argues that dynamics be particularized as well 
to spherical-wrist arms, resulting in considerable computational savings. Lastly, we examine 
simplifications in the dynamics computation due to simply-structured inertial parameters as well 
as to simply-structured kinematic parameters. 

1.1. Inverse Kinematic Positions 

A benign kinematic structure is a characteristic of most manipulators. Whereas kinematicians 
might choose to treat arbitrary linkages, manipulators are usually designed to satisfy simplifying 
kinematic criteria: 

(i) there is the kinematic equivalent of a spherical wrist, and 

(ii) neighboring joint axes arc oriented at 0° or 90° relative to each other. 

Pieper (1968) originally showed that a wrist with three intersecting axes of rotation, which is 
kinematically equivalent to a spherical wrist, is one of the configurations that leads to an analytic 
inverse kinematic position calculation. The spherical wrist allows a decomposition of the 6-dof 
inverse kinematics computation into two 3-dof kinematic computations, through a separation of 
the orientation specification from the position specification. Most 6-dof manipulators are designed 
with spherical wrists, making tins the most important case. 

If the manipulator does not satisfy one of Picper’s criteria, then in general the inverse 
kinematic positions must be found by time-consuming numerical approximation and convergence 
methods. This renders real-time control of a Cartesian trajectory infeasible (but see the discussion 
of resolved rate control below). Some manipulators that were originally designed without spherical 
wrists have been redesigned for this reason. Such manipulators might have been intended to be 
programmed by a human operator manually guiding die manipulator to desired positions with a 
teach box, so that the human operator in effect substituted for an inverse kinematics solution. As 
the manipulator’s programming was increasingly automated with higher-level computer control, 
the non-sphcrical wrist was found to be an insurmountable stumbling block to automatic real-time 
generation of Cartesian trajectories. 
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Figure 1. Cartesian position and orientation specification for a reference point on the hand of a manipulator. 
1.2. Inverse Kinematic Velocities 

Shortly after Picper’s work, Whitney (1969, 1972) proposed the Resolved Motion Rate Control 
Method, which computes the inverse kinematic velocities while avoiding the computation of the 
inverse kinematic positions. Whereas an analytic inverse kinematic position calculation is critically 
dependent on a benign manipulator configuration, the inverse kinematic velocities can be easily 
computed for any arbitrary 6-dof manipulator. If 0 = (&i,...,0e) is the vector of joint angles 
and x = is the vector specifying the position (x 1 ,x 2 ,x 3 ) = [x,y,z) and orientation 

(xi,x 5 ,x 6 ) = {6 x ,6 y ,9 z ) of the reference point on tire hand (Fig. 1), then tire forward kinematic 
positions are straightforwardly given by the transformation x = f(0). The forward kinematic 
velocities are then given by 

x = J0 (1) 

where the elements J t] of tire Jacobian J are dfi/dxj with f = (/i , .. /e)- The E-2 manipulator 
that Whitney presented in his papers did not have a spherical wrist, and Whitney proposed that 
tire trajectory be differentially approximated by solving the inverse kinematic velocities through 
inversion of (1) to avoid the intractible inverse kinematic position calculation: 

g = J-4 (2) 

Due to the prohibitive computational cost of solving (2) through 6x6 matrix inversion (Table 1), 
Whitney suggested an interpolation scheme based on precomputed inverse Jacobians evaluated at 
a few positions. 

It should be noted that the inverse Jacobian itself is often not of interest, but only the 
joint rates. Thus Gaussian elimination could have been used instead of matrix inversion, giving a 
substantial computational savings (Table 1); the numbers include evaluation of J. 

More recently, Paul (1981) proposed a method particularized to the Stanford manipulator 
which is somewhat more efficient than Gaussian elimination (Table 1); the numbers given 
are slightly larger than stated in (Paul 1981). Paul’s method involves manipulation of 4 x 4 
transformation matrices, and takes implicit advantage of the spherical-wrist configuration of the 
Stanford manipulator. 
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Method 

Multiplications 

Additions 

6x6 matrix inversion 

287 

193 

Gaussian elimination 

141 

98 

Paul (1981) 

94 

55 

Feathcrstone (1983) 

36 

20 


Table 1. Computational complexity of various methods of computing the inverse kinematic velocities 
for a 6-dof manipulator, in terms of the total number of multiplications and additions required, for the 
Stanford arm. 


Feathcrstone (1983) takes more explicit advantage of the spherical-wrist configuration of 
manipulators to propose a highly efficient method for computation of the inverse kinematic 
velocities. The method is composed of four steps. 

(i) Find the linear velocity of tire wrist from tire hand linear and angular velocity. 

(ii) Find the first three joint rates from the wrist linear velocity. 

(iii) Find the angular velocity of the hand relative to the forearm. 

(iv) Find the last three joint rates from the relative angular velocity of the hand. 

Featherstone’s method cannot be directly compared to Paul’s method because it was developed 
for a different manipulator and because the Cartesian velocity specification was different. We have 
reworked Featherstone’s method for tire Stanford manipulator with Paul’s dT 0 Cartesian velocity 
specification, where the wrist linear velocity and the time derivative of the hand orientation matrix 
are directly given. Thus the first step in Featherstone’s method is unnecessary for purposes of 
tire comparison. Table 1 shows that Featherstone’s method is almost 3 times more efficient than 
Paul’s method. Featherstone’s method works so well because he most directly takes advantage of 
the spherical wrist kinematics of robots. This is particularly important in computing the wrist joint 
rates, where Paul’s method requires 75 multiplications to Featherstone’s 22. 

1.3. Inverse Kinematic Accelerations 

As mentioned earlier, a solution to the inverse kinematic accelerations is required in the most 
general transformation between Cartesian space and joint space. For purposes of control, the joint 
accelerations are required for input into the inverse dynamics as well as for recent control law 
formulations in terms of hand acceleration. For the inverse dynamics (Johnson 1983), 

r = N- 1 (Hg + c(£<?)) (3) 

where 

r is the vector of joint torques, 

N is a matrix which reflects the recursive structure of the dynamic equations, 

H is the generalized inertia matrix, and 

c(0, 0) is the vector of centripetal and Coriolis torques. 

Several schemes have also been proposed recently that close the feedback loop around the 
hand. Adopting some of the results and notation from (Johnson 1983), these schemes include: 

1. Resolved acceleration (Luh, Walker, and Paul 1980b): 
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Method 

Multiplications 

Additions 

6x6 matrix inversion 

394 

260 

Gaussian elimination 

213 

162 

Wrist partitioning 

78 

57 


Table 2. Computational complexity of various methods of computing ihe inverse kinematic accelerations for 
a 6-dof manipulator. 


x = Xd + Kj Xrf -f K 2 (x — Xd) (4) 

2. Nonlinear control (Freund 1982): 

x = Aid + K]i-(- K 2 x (5) 

3. Cartesian impedance control (Hogan and Cotter 1982): 

x — M —1 (f e — Kr(x) — K 2 (x) + K 2 (xd)) (6) 

where 

x is the derived hand acceleration, 

x and x are the measured hand velocity and position (as determined from the joint 
angle measurements), 

x d , id, and x d are the desired or planned hand acceleration, velocity, and position, 

A and Kj are velocity gain matrices, 

K 2 is a position gain matrix, 

f e is a vector of external forces, and 

M = diag[m, m, m, ij, h,h) is a matrix which relates accelerations to generalized forces 
through the mass m of the hand and tire principal inertias h,h, / 3 . 

As can be seen, the exact formulations of the feedback laws (4)-(6) differ somewhat. Walker’s 
method is the only one requiring a planned hand acceleration x d . In Hogan’s method, the velocity 
and position gains K 1( K 2 are considered as arbitrary functions rather than just as matrices. In 
addition, Hogan also includes the external force f, and gives tire hand attributes of mass and 
inertia by the matrix multiplication M” 1 . 

Once the hand acceleration x has been derived, whether by a trajectory planner or by a 
hand-based control law, the joint accelerations can be found through differentiation of (1). 

x = Si + j| (7) 

0=J -1 (x —jg) (8) 

As in tire inverse kinematic velocity computation, solution of the inverse kinematic accelerations 
by matrix inversion (8) is very costly (Table 2). Again, since tire inverse Jacobian itself is not 
of interest, the joint accelerations are better found by solving (7) through Gaussian elimination 
(Fable 2). Until now, there had been no better method than Gaussian elimination, but it can be 
seen that the wrist-partitioning algorithm developed later is substantial more efficient. 


4 
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Figure 2. (a) First three joints of the rotary manipulator, (b) Spherical wrist of the rotary manipulator. 


The proposals of Walker, Freund, and Hogan, however, seem to require the matrix inversion 
method. The hand acceleration is incorporated into the inverse dynamics computation (3) by 
substituting for the joint accelerations (8). 

t = N~ 1 (HJ- 1 (x- j0) + c(0,0)) (9) 

If they had intended Gaussian elimination, then die inverse kinematics and dynamics should 
have been expressed in the forms (3) and (7). Because of the apparent difficulty of solving the 
inverse kinematic accelerations, Khatib (1980) proposed a hand-based feedback law which involves 
resolving hand forces through the inverse Jacobian rather titan resolving the hand accelerations. 

2. Rotary Manipulator 

We now proceed to derive an efficient formulation of the inverse kinematic accelerations. To 
demonstrate the method, we use a rotary manipulator without offsets. Other manipulator types 
are readily adapted to Featherstone’s method, although the details primarily in step 2 and slightly 
in step 3 must be reworked; the other computations remain tire same if the manipulator has a 
spherical wrist. 

The first three joints are shown in Figure 2a and the last three in Figure 2b. The joint axes 
for this manipulator are derived from the Denavit-Hartenbcrg (1955) specification. The rotation 
axis /., corresponds to joint angle 0 !+I between links t and i + 1. The internal coordinate system 

for link i is completed by defining x t from the cross product z,_i x z,, which also locates the 

origin, and y, — z t x x t . Neighboring coordinate systems are related by three parameters: 

a, is the distance between x and z, measured along x t , 

Si is the distance between x,_i and x, measured along i,—i, and 


5 
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Joint i 


s» 

Oti 

1 

0 

Sl 

7r/2 

2 

a 2 

0 

0 

3 

0 

0 

—7r/2 

4 

0 

s 4 

7t/2 

5 

0 

0 

—7t/2 

6 

0 

0 

0 


Table 3. Denavit-Hartenberg parameters for the rotary manipulator of Figure 2. 


a, is the angle between z,_i and /., measured in a righthand sense about x t . 

The values for the tliree parameters above for the rotary arm of Figure 2 are given in Table 3. 

The directions for z, and x t are chosen so that when = 0, tlren x,_j and x ; arc parallel and 
pointing in the same direction. For the rotary manipulator, this desideratum is straightforwardly 
satisfied for every coordinate sytem except 3 and 5. For coordinate system 3, we desire that the 
rotation axis z 3 point at the wrist. This means that x 3 must point in tire opposite direction from 
z 2 X z 3 , which is reflected by a 3 = — tt/2, and that the zero position for tire elbow joint is at a 
right angle pointing upward (Figure 2a). Similarly, because it is desired to point z 5 towards the 
tip of the hand, x 4 must point in the opposite direction from z 4 x z 5 , and a 5 = —7r/2 (Figure 
2b). 

The rotation matrix A t which transforms points expressed in link i coordinates to link i — 1 
coordinates is: 


'c$i 

— s6 t ccti 

sdiscii 

s6 t 

c6icai 

—cOiScti 

. 0 

SCti 

ca t 


( 10 ) 


where we have used the abbreviations s9 = sin0 and c8 — cos 9. For convenience, we list below 
the six joint transformation matrices. 



~c 9 1 

0 

s 9 j 


'c0 2 

— S0 2 

0 ■ 



~c 9 3 

0 —s 9 3 

Ai = 

s$ j 

0 

—c8\ 

A2 - 

S0 2 

c 9 2 

0 


A3 = 

s 9 s 

0 c 9 3 


. 0 

1 

0 


- 0 

0 

1 . 



. 0 - 

-1 0 


'c8 4 

0 

sd 4 


' c 9 5 

0 

~s 9 5 

- 



~c8 6 

-s 9 6 0 ' 

a 4 = 

s6 4 

0 

—084 

A5 = 

s 9 5 

0 

c 9 5 


A6 = 

= 

s 9 b 

c 9 e 0 


. 0 

1 

0 


. 0 

—1 

0 




. 0 

0 1 . 


With these transformation matrices, points on any one link may be referred to the coordinate 
system of any other link. We will denote with a left superscript which coordinate system a vector is 
referred to. Then *v is a vector referred to tire ith coordinate system, and *~ i1 v = A, ‘v is this same 
vector referred to the i — 1st coordinate system. These transformation matrices can be chained 
together to refer non-adjacent links * and j (where j > i) by defining ‘W,- = A i+1 A i+2 -• -Ay, 
where ! v = ‘Wy J v. By convention, the left superscript is omitted when referring to the base 
coordinate 0, c.g., v = °v. 

Define the following vectors, which will be useful later: 
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elbow 



Figure 3. Vector definitions between various coordinate origins of the rotary manipulator. 


p, is the vector from coordinate origin 0 to coordinate origin i, and 
p* is the vector from coordinate origin i — 1 to coordinate origin i. 

Due to the coincidence of coordinate origins 2-3 and of 4-6, many of these vectors are not distinct. 
Thus p,_i = p t and p* = 0 for i — 3, 5,6. Furthermore, Pi = p 7 . 

3. Specification of the Cartesian Trajectory 

In applying Featherstone’s method, it is necessary to have available the angular velocity and 
acceleration vectors w 6 , w 6 of the hand as well as a time history of Line {x, y, z ) Cartesian position of 
some point on the hand. Yet some of the more common Cartesian trajectory planning algorithms, 
such as those built around straight-line, constant-velocity segments (Paul 1981, Taylor 1979), yield 
instead of the angular velocity and acceleration vectors a time-varying hand orientation matrix 
W 6 . In such a case, the angular velocity and acceleration of the hand can be derived from the 
orientation matrix. 

If p 7 is a vector from the base to the point on the tip of the hand used in trajectory planning, 
and p! is the internal vector from coordinate origin 6 to this same tip point (Figure 3), then for 
a spherical-wrist arm 


Pt —p 4 4 - Wg 6 p 7 

(12) 

Pt =p 4 4 - W 6 6 p* 

(13) 

Pt =P 4 4 - W 6 6 p* 

(14) 


where the first two time derivatives of the rotation matrix W 6 must be available. In the vectorial 
representation, 


P? =p 4 + w 6 X W 6 6 P^ 

Pt =P 4 + w 6 X W 6 6 p‘ + oj 6 X (w 6 X W 6 6 p‘) 


7 


(15) 

(16) 
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Z °t 




(o) 


(b) 


Figure 4. (a) Projection of the wrist point onto the x 0 — ya plane to find 9 x . (b) Reduction to a planar 
two-link manipulator for 6 2 and 6 3 by referring positions to link 1 coordinates. 


Equating the corresponding elements, 


=W 6 Wj (17) 

die =W 6 W l + vV 6 W 6 r (18) 

where w 6 and w 6 are the matrix representations for tire cross product by u> 6 and uj 6 respectively; 
that is to say, w 6 v = w 6 x v for example. 

4. Inverse Kinematic Positions 

In applying Featherstone’s method, it is necessary to have solved for tire inverse kinematic positions 
before finding tire inverse kinematic velocities, and to have solved for the inverse kinematic 
velocities before finding the inverse kinematic accelerations. For completeness of presentation and 
to show how intermediate results from one inverse kinematic level are used in the next, we begin 
with a rederivation of the inverse kinematic positions. 

Step I: Find the wrist position. We presume that the orientation matrix W 6 for the hand and 
the position p 7 of the tip of the hand have been specified. Since 6 p*, the internal vector in link 
6 which extends from the coordinate 6 origin to the hand tip, is known, then the position of the 
wrist p 4 is given by 


P 4 = P7-W 6 6 p 7 (19) 

Step 2: Find the first three joint angles. Joint 1 is directly found from p 4 , since joints 2 and 
3 act in a plane that does not change the projection of the wrist onto the x 0 ,y 0 plane (Figure 4a): 


8 
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where the tan -1 function corresponds to the FORTRAN ATAN2 function, so that the above 
division is not actually carried out. We follow Paul (1981) in this practice because of the superior 
numerical conditioning with this inverse trigonometric function compared to others. A degeneracy 
occurs when the wrist lies on the z 0 axis, where p 4l = p 4y — 0 and 9 X can assume any value. 
Since s6 x and cd 1 are required below, they are best found with one additional transcendental 
function call. Defining r 2 = p\ x -p p\ y . 


n. riy 

c6 1 = a0 1 = — ( 21 ) 

rr 

The next two angles are easily found if p 4 is expressed in joint 1 coordinates, which reduces 
the problem to a planar two-link problem. Define 

Piu == Pi Pi — P4 (22) 

When expressed in link 1 coordinates, 




Piy 


A 4 p^ — 


' r ' 

Pujz 

. 0 . 


(23) 


where 1 p wz — 0 because the wrist point p w lies in the xi/y x plane. By the cosine rule (Figure 
4b), 


c $3 is found next. 


sO'i 


+ 4-(r 2 +rL) 

202^4 


(24) 


cds = ± yjl- (s& 3 ) 2 (25) 

"““‘"“’(S) (26) 

Depending on -which quadrant is chosen for $ 3 , one obtains an elbow up or elbow down solution. 
The angle 0 2 is found by expressing 1 p^ in terms of die joint axis vectors. 


p w —a 2 x 2 -p S 4 Z 3 


Pw 


a 2 c 9 2 — s 4 s($ 2 -p $3) 
a 2 sd2 -p s 4 c($2 -p $3) 


0 


( 27 ) 



9 
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Solving by simultaneous equations, 


s$ ~ ( 34 - 5 ^ 3 )) — 'Vwx{sic 9 z) 

°2 + s 4 — 2a 2 (s 4 50 3 ) 

Cd 2 = ^ Pwx S0 2( S 'i c0 3) 

a 2 — (s 4 s9 3 ) 



(28) 

(29) 

(30) 


where the products in tire parentheses are computed only once. 

Step 3: Find the hand orientation relative to the forearm. The forearm orientation is given by 
W 3 = AiA 2 A 3 , so that die hand orientation relative to the forearm, 3 W 6 = A 4 A 5 A 6 , is given by 


3 W 6 = \vrw 6 


(31) 


Because they yield intermediate results which are useful later, we present the partial matrix 


multiplications AjA 2 

and (AiA; 

>)^3- 



'c0i c9 2 — 

C0i S0 2 

50x 

AiA 2 — 

S01C02 — 

50150 2 

— C0i 


. 50 2 

C0 2 

0 


■O0 3 (c0ic0 2 ' 

— S0 3 (c0iS0 2 ) 


(Aj A 2 )A 3 


c0 3 (50i c0 2 ) — s9 3 (s9is6 2 ) 
s(9 2 + 63 ) 


-s9i — s0 3 (c0ic0 2 ) — c9 3 (c9is9 2 ) 
c9 4 —50 3 (50ic0 2 )— c0 3 (50i50 2 ) 

0 c(02 + ^ 3 ) 


(32) 


(33) 


where the sine/cosine products in the parentheses are computed only once and the 31 and 33 
elements in (33) are computed from the expansion. Finally, we presume 3 W 6 has been computed 
from a knowledge of W 6 and 0i,0 2 ,0 3 , and that its elements are v) i3 . 

Step 4: Find the last three joint angles. Joint angles 0 A and 0 6 can be found from the elements 
of 3 W 6 , which are identified by some matrix manipulations. 


A4A5 

3 w 6 a[ 


'cd 4 c9 3 —s0 4 — c9 4 S$^ 

s9 4 c9 5 c9 4 — s0 4 s0 5 

. $9$ 0 c0 5 . 

'w u c9 6 — w 12 sd e w n s9 e -f w 12 c9 6 
w 2 ic9 6 — w 22 s9 e w 2i s9 6 -f wivcOq 
.1031006 — W 32 s9 6 W31 s0 6 + w 32 c9 6 


W13 

w 23 

W33 


(34) 



Since A 4 A 5 = 3 W 6 A|\ we find by selecting elements 13 and 23 that 
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(36) 


Because of the multiplication factor s0 5 , there are two possible solutions to this equation; the 
quadrant must be chosen by some other criterion such as continuity. Note the singularity when 
sin 0 5 = 0, causing the z 3 and z 5 axes to line up. There is no numerically-sound shortcut to finding 
s0 4 and c0 4 , required below, so that 2 more transcendental function calls are needed. 

Similarly, 0 5 and 0 6 can be found by equating A 5 A 6 and A[ 3 W 6 : 


■ C0 5 C0 6 

—C 0 5 S 0 6 

— S0 5 




S0 5 C0 6 —505S0 6 

C0 5 



(37) 

-~S 0 6 

— C0 6 

0 




~W ilC0 4 -f- W21 50 4 

Wi 2 C 0 i + W 22 S0 4 

W n C 04 -f 1023-5^4 


W 3 1 

'“'32 

W33 

(38) 

,W\ 4 S0 4 — 1021 C04 

W 12 S 0 4 - 

- W22^04 

W n S 04 — W23C64 ■ 



0 5 can be found from the 13 and 23 elements of these last matrices and the value of 0 4 . 


S0 5 — — 10:3 004 — W 2 3S0i 

C0 5 =W3 S 

By selecting the 31 and 32 elements of these matrices, 


(39) 

(40) 

(41) 


506 = — W n S0 4 -f W21C04 (42) 


C06 = — W 12 S0i -f W 2 2C0 4 (43) 



This particular method of computing 0 6 has been chosen because it yields s0 6 and c0 6 , and yields 
0 6 unambiguously. 

4.1. Computational complexity 

The total computational complexity for steps 1-4 comes to 64 multiplications, 38 additions, and 
10 transcendental function calls, broken down as follows: 

Step 1: 9 multiples and 9 additions. 

Step 2: 15 multiplications, 9 additions, and 5 transcendental function calls. In deriving the additions 
and multiplications, we note that for 0 3 the subexpressions and 2a 2 s 4 can be precomputed. 

Step 3: 34 multiplications and 17 additions. Note that only seven elements of 3 W 6 are required, 
namely ton,W 12 , iox 3 , w 2 i ,^221 w 2 Si w 33 - 

Step 4: 6 multiplications, 3 additions, and 5 transcendental function calls. 
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5. Inverse Kinematic Velocities 

Step 1: Find the wrist linear velocity. The wrist and hand tip linear velocities are related by 

P 4 — P 7 Wg X P7 

Step 2: Find the first three joint velocities. The wrist position is given by 


p 4 — SiZ q T~ a 2 X 2 + S 4 Z 3 


Differentiating, 


j >4 = <±>2 X <l2 x 2 T" X S4Z3 

When evaluated in link 2 coordinates (Appendix 1), 


Pt 


— S 4 C0 3 (^2 + ^3) 
02^2 — SiSd 3(62 + ^3) 
01 *Pwx 


where 2 p 4 = (AiA 2 ) r p 4 . Solving, 


71 = 


P42 


9* 


Ipwx 

2 PiyC0S — 2 P4 X S6 

a 2 c 0 3 

2 Pix — 

s<c6 3 


(45) 

(46) 

(47) 

(48) 


(49) 


Step 3: Find the angular velocity of the hand relative to the forearm The hand angular 
velocity relative to the forearm, u h , is given by 


(50) 


This is best evaluated in joint 4 coordinates, accomplished as follows: 


0is(9 2 + 0 3 ) 

S W 3 — —(0 2 -(- 0 3 ) 

.01 c{02 + ^3)J 
3 w 6 =(AiA 2 A 3 )' r w 6 


3 a; - 3 


w R — “w. 


vr-h — 


4 , _A T 3 

U h —A 4 w h 


' CJh, x C04 ~T 2 UJhyS04 

Z U hz 

L 3 W^S0 4 — 3 WhyC04\ 


(51) 

(52) 

(53) 

(54) 
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Step 4: Find the last three joint velocities. The hand angular velocity relative to the forearm 
is also given by 

<±Lh = Z 3#4 + *4^5 + z 5$6 (55) 

Expressed in joint 4 coordinates, we find that 


Solving, 


z 3 = V4 



L 0 J 



— 9qs6^ 

#4 T* $6 C ^5 

h 


(56) 

(57) 


(58) 


#5 = 


(59) 


h 


4 Uhx 

sOs 


(60) 


— 4 Uhy — $ 6^5 


(61) 


5.1. Computation Complexity 

The total for steps 1-4 is 37 multiplications and 25 additions. In arriving at these numbers, 
we presume to have the results of the inverse kinematic position computation available. The 
breakdown is as follows: 

Step 1: 6 multiplications and 6 additions. 

Step 2: 15 multiplications and 7 additions. 

Step 3: 14 multiplications and 11 additions. 

Step 4: 2 multiplications and 1 addition. 

6. Inverse Kinematic Accelerations 

Step 1: Find the wrist linear acceleration. This is readily found as 

p 4 = p 7 - w 6 X p* - w 6 X (we X Pj) (62) 

Step 2: Find the first three joint accelerations. Differentiating (47), and noting that p w == p 4 , 
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P 4 — 0 2 X P tt . + ^2 ^ p4 — «4^3 X 3 — S 4 0 3 OJ s X X 3 

Expressed in joint 2 coordinates, this equation evaluates to (Appendix II) 


— ^iC0 3 (O 2 + ^ 3 ) 
°2^2 — S 4 S0 3 (0 2 + 0 3 ) 

= 2 p 4 - 2 w 2 X 2 Pt + 

s 4 0 3 s0 3 3 w 3y 
~S 4 0 3 C0 3 3 W 3 y 

Pwx'01 


S 4 ^3 3 W 32 — 1 p W yb X 0 2 . 


Defining the right side as 2 ii 4 , the joint accelerations can now be found. 


(63) 


(64) 




02 

h 


_ UAz 
*Pwx 

2 U 4y c0 3 — 2 u 4x s0 3 
a 2 c6 3 

2 U 4x “f S 4 C0 3 '$2 
S 4 C0 3 


(65) 

( 66 ) 

(67) 


Step 3: Find the hand angular acceleration relative to the forearm. By differentiating (50), we 
find that 

Wfe = Wg — w 3 — W 3 X W h (68) 

The angular acceleration w 3 is found in link 3 coordinates from the previous results. 


l 0 4 = x yi8i 


UJ 0 


0 4 02 

0i 

. h . 


2 • _ A T 1 • 

! iL 2 —™2 bJ 2 


0.3 — 0.2 + 2 ' £ 2 03 ~ t ~ 

3,-, _aT2,- 
W 3 —A 3 W 3 

Evaluating in link 4 coordinates, 


0103°02 

—010 3 S0 2 

0 


(69) 


(70) 

(71) 

(72) 




3 d> 6 =(A 1 A 2 A 3 ) T u; 6 
4 Oh — A 4 ( 3 Wg 3 w 3 3 w 3 X 3 caJ 

14 


(73) 

(74) 
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Kinematic Parameter Multiplications Additions 

Joint Angles 

64 

38 

Joint Velocities 

37 

25 

Joint Accelerations 

78 

57 

Combined Total 

179 

120 


Table 4. Computational complexity for evaluation of the inverse kinematic positions, velocities, and 
accelerations. 


Step 4: Find the last three joint accelerations. Written in terms of the last three angular 
accelerations, the relative hand acceleration is 


tkh — z 3$4 4" z 4#5 4" z 5$6 4* z 3$4 X *4^5 4" ( z 3^4 4" z 4^5) X Z$06 (75) 


When evaluated in joint 4 coordinates, 


% = 

04^5 — 0506^05 
— 0 5 0qS0 5 

+ 

— 0&S0 5 

0&C05 + 04 


040qS0^ 


. h . 


(76) 


The joint accelerations can now be found. 


>5 4 &hx — 0405 4“ 

=-- 

SO 5 

= 4 djhz — 040$S05 
04 — i d>h y 4 " 04>06 s ^5 — 06C&5 


(77) 

(78) 

(79) 


6.1. Computational Complexity 

The total is 78 multiplications and 57 additions, using the results of the previous inverse calculations. 
This is constituted as follows: 

Step I: 12 multiplications and 12 additions. Here w 6 x p? is already known. 

Step 2: 29 multiplications and 17 additions. It is required to compute 2 w 2 here. 

Step 3: 30 multiplications and 23 additions. 

Step 4: 8 multiplications and 5 additions. 

Table 4 summarizes tire results for computation of the inverse kinematic positions, velocities, and 
accelerations. 
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link i 



Figure 5. An illustration of vectors defined in the text 


7. Dynamics Computation 

In the recursive Newton-Euler dynamics formulation, which is the most efficient one (Hollerbach 
1980), a kinematics computation precedes application of the Newton-Euler equations. Because 
of the simplified kinematics of spherical wrist robots, the kinematic portion of the dynamics 
computation is simplified as well. In addition, the inverse kinematic computations produce some 
of tire same quantities as does the inverse dynamics computation. Therefore a combined inverse 
kinematic - inverse dynamics computation would save some operations in the latter computation. 

7.1. Recursive Newton-Euler Dynamics 

The recursive Newton-Euler computation of the dynamics proceeds by a two-step recursive 
procedure (Luh, Walker, and Paul 1980a). First, the angular velocities and accelerations of each 
link are computed along with the linear acceleration of each joint. 


'w t =Af(* 1 w t —i +' 1 z t—i^t) (80) 

'fki =Af(* 1 w,_i -j- 1 x z t _i+ * 1 w i _ 1 X ' u i l ~iO l ) (81) 

% =Af !),__! + 'w, X 'p* + % X CUi X 'p‘) (82) 

where these particular equations presume a rotary joint manipulator only. From these results, the 
Newton-Euler equations may be applied to find the net force and torque acting at each link. 


% =i P. + x 'r‘ -f 'w, x ('y, X l r‘) 
'f, =m t ‘r l 

% =*V& + k X (%%) 
where previously undefined terms are (Figure 5): 


(83) 

(84) 

(85) 
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‘r* is a vector from coordinate system i to tire center of mass of link *, 
% is the acceleration of the center of mass of link i, 

% is the net force on link i, 

'n, is the net torque on link i, and 

! I t is tire inertia tensor of link i about its center of mass. 

Secondly, the forces and torques are propagated from tire tip to the base. 


%-x,i - t f t + A t+1 '+ 1 f t , t+1 

%-i.i ='n; + A i+ i ,+1 n M+ i + (*'p* + ! 'r‘) X ‘f, + ; p* X 


_ _ i — 1 i — 

—— Tj\ —i * lit — i t 


( 86 ) 

(87) 

( 88 ) 


where 

%—i t i is the force exerted on link i by link i — 1, 

is the moment exerted on link i by link i — 1, and 
r t is the input torque at joint i. 

Evaluation of these dynamics for a general 6-rotary-joint manipulator requires 688 multiplications 
and 558 additions (a slightly lower figure than given in (Holierbach 1980)). 

7.2. Dynamics Particularized to a Spherical-Wrist Manipulator 

By taking advantage of the simple kinematic structure of the spherical-wrist, rotary manipulator, 
the dynamic complexity is reduced to 448 multiplications and 361 additions. The breakdown for 
the savings is as follows. 

A.: 116 multiplications and 87 additions are saved. A matrix multiply now takes 4 multiplications 
and 2 additions instead of 8 multiplications and 5 additions because each a, is either 0 
or ±7r/2. There are 29 applications of the link transformation matrices A, for the six-joint 
manipulator. 

p t : an additional 92 multiplications and 78 additions are saved. This comes about because (i) 
p t = Pj_! for i = 1,3,5,6, although the term % — Af 1 p,_ 1 must still be evaluated; 
and (ii) P 2 = z t a 2 , p^ — z 3 s 4 simplify some of tire cross products. 

an additional 32 multiplications and 32 additions saved. As above, this comes about from 
the simple p* vectors when evaluating ! p* x 

w 0 , w 0 : If the assumption of a non-spinning base is made, i.e., w 0 — w 0 = 0, then an additional 
40 multiplications and 37 additions is saved in evaluating 'wj, 2 w 2 , 1 w 1 , 2 w 2 , ‘ri, and J ni. 

Counting the non-spinning base assumption, the dynamic complexity is reduced to 408 multiplications 
and 324 additions, a savings of 280 multiplications and 234 additions. It should be noted that if 
the A, matrices are not provided to the dynamic computation, then an additional 4 multiplications 
are required to form each matrix in the general case. T his would be unnecessary for the particular 
manipulator considered here. 
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7.3. Inverse Kinematics Considered with Inverse Dynamics 

In computing the inverse kinematics, there is an additional modest savings in the inverse dynamics 
of 40 multiplications and 28 additions, bringing the total dynamics complexity down to 368 
multiplications and 296 additions. This results from tire precomputation of tire following quantities. 

1 Wi ) 2 w 2j 3 w 3 : 6 multiplications and 3 additions are saved. 

Wi, 2 <ii 2 , W 3 : 10 multiplications and 7 additions are saved. 

4 p 4 : 4 multiplications and 4 additions are saved. Since 2 p 4 is known, then 


3 P 4 — Aj 2 p 4 , 4 p 4 = Aj 3 p 4 

requires 8 multiplications and 4 additions for evaluation instead of 12 multiplications and 8 
additions from (82). 

2 p 2 : 8 multiplications and 6 additions are saved. It is not necessary to compute p 2 because 2 r 2 
can be computed without it and p 4 is known. From (82) and (83), 


% = A|’ 1 p 1 + 2 w 2 x ( 2 p 2 + 2 h) + 2 w 2 x ( 2 w 2 x ( 2 p 2 + 2 h)) 


(89) 


3 p 3 : 4 multiplications and 2 additions saved. Similarly, it is not necessary to compute p 3 because 
3 r 3 can be computed from 3 p 4 . 


3 P 3 = 3 P 4 - Ws X s 4 3 z 3 - W 3 x ( 3 w 3 X s 4 3 zs) (90) 

3 'h = 3 P 4 + Ws X ( 3 r* — s 4 3 z 3 ) + 3 w 3 X (W 3 X ( 3 r 3 — s 4 s z 3 )) (91) 

where 3 r 3 — s 4 s z 3 is fixed and is precomputed. 

4 w 4 , 5 w 5 , 6 w 6 : 3 multiplications and 2 additions saved. This results from calculating 6 w e directly 
from w 6 and the components of 4 w /l separately. 


We =Wjw 6 


(92) 

W 6 =A 6 6 w 6 


(93) 

W 6 =A 5 5 w 6 


(94) 

W 3 =A[ 3 o) 3 


(95) 

Wfc = W 6 - 

4 — 3 

(96) 

W 4 = 4 ~3 + 

4 y 4 0 4 

(97) 

W 5 = W 6 - 

**5*6 

(98) 


Ordinarily, 24 multiplications and 19 additions would have been required to calculate these 
quantities. 
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4 w 4) s M 5 , 6 w 6 : 5 multiplications and 4 additions saved. Similarly, some savings can be accomplished 
by computing °w 6 from w 6 and tlie components of 4 d> h separately. 




6 w 6 =W 6 7 ’w 6 (99) 

5 w 6 —A 6 6 cag (100) 

4 ^ 6 =A 5 % (101) 

4 w 3 —A.J 3 w 3 (102) 

A <k h = 4 w 6 — 4 w 3 — 4 W 3 X *u h (103) 

4 W 4 = 4 w 3 -f- 4 y 4 0 4 -f- 4 w 3 x 4 y 4 0 4 (104) 

®W5 = 5 w 6 — 5 Z506 — 5 fc!.5 X 5 *5^6 (105) 


Ordinarily, 36 multiplications and 31 additions are required for these quantities. 

7.4. Simplified Inertial Parameters 

No presumptions were made above about the inertial parameters of the manipulator, namely the 
center of gravity, the principal inertias, and the orientation of the principal axes of inertia for 
each link. While tire kinematic structure of manipulators is deliberate, the inertial parameters are 
seldom a factor in design. Therefore a simplifying set of inertial parameters cannot be assumed a 
priori. Paul (1981) considered one way in which the dynamics might be simplified if a particular 
set were assumed. In the present study, if the center of gravity and the principal axes lined up 
with the internal link coordinate system, an additional simplification of 174 multiplications and 
168 additions in the dynamics would result. 

l r l : 60 multiplications and 54 additions are saved. If ? r* lies along a coordinate axis, then each 
% requires only 8 multiplications and 6 additions for evaluation. 

24 multiplications and 24 additions are saved. The term ('p* -f *r‘) x now requires 2 
multiplications and 2 additions for its formation and its addition, where *p* -f *r* lies along 
an axis. 

'np. 90 multiplications and 90 additions are saved. If T, is aligned with the coordinate axes, then 
it is diagonal. Three additions are saved in the x T t ! w t term because the principal inertia 
differences can be precomputed. 

If the simplified inertial parameters were considered along with all the other computational 
savings, then tire dynamics would require only 194 multiplications and 138 additions. Table 5 
summarizes the dynamics complexity for the various kinematic and dynamic conditions that have 
been considered above. It appears that the dynamic equations are even more approachable from 
a computational standpoint than had been considered heretofore (Hollerbach 1980). Under the 
best conditions, an exact evaluation of the dynamics has roughly the same complexity as a full 
evaluation of the inverse kinematics (Table 4). 
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Condition 

Multiplications 

Additions 

General Rotary Manipulator 

688 

558 

Spherical Wrist Manipulator 

408 

324 

Precomputed Inverse Kinematics 

368 

296 

Simplified Inertial Parameters 

194 

138 


Table 5. Dynamics computation complexity for a 6-dof manipulator under various conditions. 


8. Discussion and Summary 

An algorithm for calculation of the inverse kinematic accelerations has been presented, which 
is the most efficient one to date. Based on a method developed by Fcatherstone (1983), the 
algorithm directly takes advantage of the structure of spherical-wrist manipulators to decompose 
the 6-dof inverse problem into two 3-dof inverse problems through a 4-step procedure. The 
resultant equations for tire first 3 joints, but not the last three, arc particular to a given manipulator 
structure, but tire technique is easily extended to other structures. One could envision a catalog of 
equations for the most common manipulator configurations. 

Spherical wrists are the most important case for current 6-dof robots and are becoming 
standard. An additional simplifying kinematic criterion, namely that neighboring joint axes are 
oriented parallel or orthogonal to each other, is almost always followed in manipulator design and 
aids the solution to the two 3-dof kinematic problems. 

These two kinematic criteria have a simplifying effect on tire dynamic equations as well. 
Since in the recursive Newton-Euler equations (Luh, Walker, and Paul 1980a) tire computations 
are carried out in internal link coordinates, the transformations between neighboring links are 
simplified. Angular velocities and accelerations as well as linear accelerations of tire link origins 
are calculated more simply, even as they are in the inverse kinematics computation. Because the 
inverse kinematic acceleration algorithm produces some of these last-mentioned vectors as well, 
then a combined inverse kinematic accelcration/inverse dynamics computation results in some 
savings for the inverse dynamics calculation. 

The implication of these algorithms is that the computational requirement for either the 
inverse kinematics or tire inverse dynamics is low enough to warrant easy real-time implementation. 
Recent control algorithms based on derived hand acceleration (Luh, Walker, and Paul 1980b, 
Freund 1982, Hogan and Cotter 1982) are made computationally feasible, making these algorithms 
more competitive with schemes which close force loops around the hand to avoid die inverse 
kinematic problem (Khatib 1980). The recursive Newton-Euler formulation is made even more 
efficient; if simplifying inertial parameters are assumed, the dynamics complexity roughly equals 
the inverse kinematics complexity. It would seem that a general Cartesian trajectory control ability, 
involving full inverse kinematics and dynamics as well as sophisticated hand-based control laws, 
is within reach. 
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Appendix I 

The wrist velocity is given by 

p4 = y .2 X ^ 2^2 T" y .3 X S 4 Z 3 (106) 


where 


Substituting above. 


W 3 = + z 2#3 


(107) 


P 4 — W 2 X ( 02^2 + «4 z 3) "t" $3 Z 2 X Z 3 S 4 (106) 

Noting that p^ = a 2 x 2 + s 4 z 3 and x 3 = -z 2 X z 3 , 

p 4 = w 2 X p w — a 4 0 3 x 3 (109) 

where 

w 2 = Ml ~b *1^2 (110) 

The wrist velocity is best evaluated in joint 2 coordinates, a reflection of the regular kinematic 
structure of the first 3 joints. Since the rotation axes are either perpendicular or parallel to each 
other, then a rotation axis in one coordinate system will also be a major axis in the next coordinate 
system. Link 2 coordinates arc the most convenient because they are situated in the middle of the 
coordinate systems. Starting first with tire angular velocity 2 w 2 , 



's6 2 ' 

2 yl = 

C$2 


. 0 - 

2 Z2 


$lS$2 


6\c0 2 



The vector p w in link 2 coordinates is 



°2 — S 4 S0 3 
S 4 CO 3 
0 


( 111 ) 

( 112 ) 

(113) 


(114) 


Thus the cross product term in (109) evaluates to 
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~02 2 Pwy 


— 02 (s 4 C0 3 ) 

02 2 Pwx 

= 

0 2 (a2,— s 4 s^3) 

m 0l{ PwyS0 2 PujxC$ 2 )_ 


. 01 Pwx 


where it is noticed that 1 p wx — —( 2 p W ys9 2 — 2 p W xC0 2 ). Finally, 




2 p 4 = 2 w 2 x — nh 2 X 3 
2 X 3 = (c0 3) S0 3j 0) 

Collecting all terms, (79) becomes 


P4 


—S 4 C0 3 (#2 + h) 
0-202 — S 4 S 0 3 {02 " 4 " ^ 3 ) 

01*Pwx 


Appendix II 

The wrist acceleration in link 2 coordinates is 


2 P 4 = 2 W 2 X 2 Pu, + 2 W 2 X 2 P 4 — S40 3 2 X 3 — S4O3 2 w 3 X 2 x 3 
Starting with the first term on the right, 


1 W 2 =[0102, $1) $ 2 ) 

Pw == ( Pwxi Pwyj 0) 


1 W 2 X 



— h X Pv>y 
02 X Pwx 


Pl02 X Pwy ~ h X Pwx J 


Aj( x w 2 X x p„,) = 


02( 1 PwxS02 — 1 PwyC02) 
02{ l PwxC02 + l pwyS02 ) 
0102 Pwy ~~ ^1 Pwx . 


This expression can be further simplified by noting that 


1 p wx S02 — X PwyC02 = — 5 4 C0 3 
— 02 — 


(115) 


(116) 

(117) 


(118) 


(119) 

( 120 ) 
( 121 ) 

( 122 ) 

(123) 


(124) 

(125) 


/**\ 
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0 


The last term on the right is evaluated as follows: 


3 w 3 X 3 x 3 = 


0 - 
3, , 

z 


L— 3 W3vJ 


A 3 ( 3 w 3 X 



■ S$ 3 S UJ 3 y " 

—c9 3 3 u 3y 

3, , 

U — U 3z 


(126) 


(127) 


Collecting all unknowns on one side, 


—S 4 C0 3 0 2 — S 4 C0 3 0 3 
(a 2 — s 4 s 0 3 )^2 — S4SM3 

= 2 P 4 - V 2 X 2 P4 + 

S 4 0 3 S0 3 S W 3 y 
—S 4 0 3 C0 3 3 U> 3y 

- Pwx^l 


—s 4 0 3 3 w 3z — 1 p wy 6 x 0 2 _ 


(128) 
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